//
// Created by yunnan xu on 2020/12/9.
//

#include "Basic.h"
#include "ext.hpp"
#include "glm.hpp"
#include "gtx/vector_angle.hpp"
#include "gtx/projection.hpp"

namespace Phy
{

    void LOGMat(const char * name, glm::mat4x4 mat)
    {
        PHYLOGI("\n matrix %s \n %f %f %f %f \n %f %f %f %f \n %f %f %f %f \n %f %f %f %f \n",
            name,
            mat[0][0], mat[0][1], mat[0][2], mat[0][3],
            mat[1][0], mat[1][1], mat[1][2], mat[1][3],
            mat[2][0], mat[2][1], mat[2][2], mat[2][3],
            mat[3][0], mat[3][1], mat[3][2], mat[3][3]);
    }

    void LOGMat(const char * name, glm::mat3x3 mat)
    {
        PHYLOGI("\n matrix %s \n %f %f %f \n %f %f %f \n %f %f %f \n",
                name,
                mat[0][0], mat[0][1], mat[0][2],
                mat[1][0], mat[1][1], mat[1][2],
                mat[2][0], mat[2][1], mat[2][2]);
    }

    void LOGVec(const char * name, glm::vec3 vec)
    {
        PHYLOGI("\n vector %s \n %f %f %f \n",
                name,
                vec[0], vec[1], vec[2]);
    }


    bool IsPointIn(const Segment & s, const Point & p)
    {
        return abs(glm::distance(s.pos1, s.pos2) - glm::distance(s.pos1, p.pos) -glm::distance(s.pos1, p.pos)) < EPS;
    }

    bool IsPointIn(const Ray & s, const Point & p)
    {
        glm::vec3 to_p = p.pos - s.pos;
        float angle = glm::angle(to_p, s.dir);
        return angle < EPS && angle > -EPS;
    }

    bool IsPointIn(const Line & s, const Point & p)
    {
        glm::vec3 to_p = p.pos - s.pos;
        float angle = glm::angle(to_p, s.dir);
        return (angle < EPS && angle > -EPS) || (angle < PI + EPS && angle > PI - EPS);
    }

    bool IsPointIn(const Plane & s, const Point & p)
    {
        glm::vec3 to_p = - s.pos + p.pos;
        glm::vec3 cross_vec = glm::cross(s.dir1, s.dir2);
        cross_vec = glm::normalize(cross_vec);
        glm::vec3 proj_vec = glm::proj(to_p, cross_vec);
        return glm::length(proj_vec) < EPS;
    }

    bool IsPointIn(const Triangle & s, const Point & p)
    {
        glm::vec3 to_p_1 = p.pos - s.pos1;
        glm::vec3 to_p_2 = p.pos - s.pos2;
        glm::vec3 to_p_3 = p.pos - s.pos3;

        float area1 = glm::length(glm::cross(to_p_1, to_p_2));
        float area2 = glm::length(glm::cross(to_p_2, to_p_3));
        float area3 = glm::length(glm::cross(to_p_3, to_p_1));
        float tot_area = glm::length(glm::cross(s.pos1 - s.pos2, s.pos1 - s.pos3));

        return abs(tot_area - area1 - area2 - area3) < EPS;
    }

    bool IsPointIn(const Sphere & s, const Point & p)
    {
        return glm::distance(s.center, p.pos) < s.radius;
    }

    bool IsPointIn(const Tetrahedron & s, const Point & p)
    {
        for (int i = 0; i < 4; i++)
        {
            Plane plane;
            plane.pos = s.pos[i];
            plane.dir1 = s.pos[(i + 1) % 4] - s.pos[i];
            plane.dir2 = s.pos[(i + 2) % 4] - s.pos[i];

            Point other(s.pos[(i + 3) % 4]);

            glm::vec3 to_p = - plane.pos + p.pos;
            glm::vec3 to_other = other.pos - plane.pos;

            glm::vec3 cross_vec = glm::cross(plane.dir1, plane.dir2);
            cross_vec = glm::normalize(cross_vec);
            glm::vec3 proj_vec = glm::proj(to_p, cross_vec);

            if (glm::dot(proj_vec, to_other) < 0) return false;
        }
        return true;
    }

    std::tuple<glm::vec3, float, bool> Dist(const Plane & s, const Point & p)
    {
        glm::vec3 to_p = - s.pos + p.pos;
        glm::vec3 cross_vec = glm::cross(s.dir1, s.dir2);
        cross_vec = glm::normalize(cross_vec);
        glm::vec3 proj_vec = glm::proj(to_p, cross_vec);

        return std::make_tuple<glm::vec3, float, bool>(glm::normalize(proj_vec), glm::length(proj_vec), glm::dot(proj_vec, proj_vec) > 0);
    }


    bool CheckIfTwoPointsInSameSideOfPlane(Point p1, Point p2, Plane plane)
    {
        glm::vec3 to_p1 = plane.pos - p1.pos;
        glm::vec3 to_p2 = plane.pos - p2.pos;

        glm::vec3 cross_vec = glm::cross(plane.dir1, plane.dir2);
        cross_vec = glm::normalize(cross_vec);

        return glm::dot(cross_vec, to_p1) * glm::dot(cross_vec, to_p2) > 0;
    }

    glm::vec3 CalcNormal(const Triangle & t)
    {
        glm::vec3 dir12 = t.pos2 - t.pos1;
        glm::vec3 dir13 = t.pos3 - t.pos1;

        auto norm = glm::cross(dir12, dir13);
        return glm::normalize(norm);
    }
}